#ifndef __BBARM_CARMBOX_H_
#define __BBARM_CARMBOX_H_

#include <robot/robot.h>
#include <robot/kuka/enums.h>
#include <blackbox/bases/bloopbox.h>
#include <blackbox/cdatain.h>
#include <blackbox/cdataout.h>
#include <blackbox/carrayin.h>
#include <blackbox/carrayout.h>
#include <blackbox/cmethod.h>

#include <buola/io/csocketacceptor.h>
#include <buola/xml.h>

namespace buola { namespace robot { namespace kuka {

using namespace buola;

class CArmBox : public blackbox::BLoopBox
{
public:
    CArmBox();

private:
    void InitOutPacket();
    
    bool ReadXML();
    void CalcCorrection();
    bool SendXML();
    
    void OnLoop();

private:
    void OnArmConnected(io::PSocket pSocket);
    void SetMaxSpeed(double pLinear,double pRot);
    
public:
    blackbox::CArrayOut<double> oJoints;
    blackbox::CDataOut<mat::CVec3d> oPosition;
    blackbox::CDataOut<mat::CQuat_d> oOrientation;
    blackbox::CArrayIn<double> iJoints;
    blackbox::CDataIn<mat::CVec3d> iPosition;
    blackbox::CDataIn<mat::CQuat_d> iOrientation;
    blackbox::CEnablerIn<EArmMode> eMode;
    blackbox::CDataOut<int> oStatus;
    blackbox::CMethod<void(double,double)> mSetMaxSpeed;

private:
    std::unique_ptr<io::CSocketAcceptor> mAcceptor;
    io::PSocket mKUKASocket;
    condition_variable mKUKACondVar;
    mutex mKUKAMutex;
    int32_t mKUKAIPOC;
    
    mat::CVecNd<6> mActualJoints;		//actual position of the joints
    mat::CVecNd<6> mGoalJoints;		//actual goal as set IN the robot
    mat::CVecNd<6> mActualVelocity;	//difference in position between two frames
    mat::CVecNd<6> mCorrection;

    mat::CVec3d mActualPos;
    mat::CQuat_d mActualRot;

    mat::CVec3d mGoalPos;		//goal position as set in the robot
    mat::CQuat_d mGoalRot;
    
    struct
    {
        CXMLDoc mDoc;
        CXMLNode mAKorr;
        CXMLNode mIPOC;
    } mOutPacket;

    struct
    {
        double mAlpha;
        double mSoftThreshold;
        double mHardThreshold;
        mat::CVecNd<6> mMaxVelocity; //degrees/sec
        mat::CVecNd<6> mMaxAcceleration;	//degrees/sec^2
        double mMaxLinearSpeed;
        double mMaxRotSpeed;
    } mParams;
};

/*namespace kuka*/ } /*namespace robot*/ } /*namespace buola*/ }

#endif
